(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[     69042,       1899]
NotebookOptionsPosition[     68236,       1870]
NotebookOutlinePosition[     68594,       1886]
CellTagsIndexPosition[     68551,       1883]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{
Cell[BoxData[
 RowBox[{
  RowBox[{"(*", "NAO", "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"use", " ", "rT", " ", "instead", " ", "of", " ", "LT"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", "initialization", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/output_v2/models/NAO\>\"", "]"}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{"<<", "RobotLinks.m"}], "\n", 
   RowBox[{"<<", "Linearize.m"}]}]}]], "Input",
 CellChangeTimes->{{3.525028795242095*^9, 3.525028798347091*^9}, {
   3.525029058469722*^9, 3.525029081601416*^9}, {3.527366972613077*^9, 
   3.527367010701981*^9}, {3.527367079389505*^9, 3.527367091508872*^9}, {
   3.5273674161254797`*^9, 3.52736742619272*^9}, 3.52737014603018*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"model", " ", "specification"}], "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"robotInfo", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
        "Lc", ",", "Lt", ",", "LT", ",", "rc", ",", "rt", ",", "rT", ",", 
         "mh", ",", "mt", ",", "mc", ",", "mf", ",", "g"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
        "380", ",", "273.60", ",", "0", ",", "282.37", ",", "129.66", ",", 
         "5.27", ",", "746.56", ",", "940.74", ",", "213.79", ",", "0", ",", 
         "9810"}], "}"}]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"constsubs", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Lc", "\[Rule]", 
        FractionBox["5137", "50000"]}], ",", 
       RowBox[{"Lt", "\[Rule]", 
        FractionBox["1", "10"]}], ",", 
       RowBox[{"LT", "\[Rule]", 
        FractionBox["30218105", "209827159"]}], ",", 
       RowBox[{"ha", "\[Rule]", 
        FractionBox["4511", "100000"]}], ",", 
       RowBox[{"hf", "\[Rule]", 
        FractionBox["37116100", "1208051301"]}], ",", 
       RowBox[{"lf", "\[Rule]", 
        FractionBox["357", "25000"]}], ",", 
       RowBox[{"lh", "\[Rule]", 
        FractionBox["6", "125"]}], ",", 
       RowBox[{"lt", "\[Rule]", 
        FractionBox["261", "2500"]}], ",", 
       RowBox[{"mh", "\[Rule]", 
        FractionBox["130763", "50000"]}], ",", 
       RowBox[{"mt", "\[Rule]", 
        FractionBox["3333", "6250"]}], ",", 
       RowBox[{"mc", "\[Rule]", 
        FractionBox["29951", "50000"]}], ",", 
       RowBox[{"mf", "\[Rule]", 
        FractionBox["7549", "25000"]}], ",", 
       RowBox[{"rc", "\[Rule]", 
        FractionBox["6322731", "325193729"]}], ",", 
       RowBox[{"rt", "\[Rule]", 
        FractionBox["66471778", "1134333797"]}], ",", 
       RowBox[{"rT", "\[Rule]", 
        FractionBox["30218105", "209827159"]}], ",", 
       RowBox[{"g", "\[Rule]", 
        FractionBox["981", "100"]}]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"ndof", " ", "=", " ", "5"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"mm", "=", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc"}], "}"}], "/.",
       "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"statesubs", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
          RowBox[{"x", "[", "i", "]"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "i"], "\[Prime]",
            MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
          RowBox[{"x", "[", 
           RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"define", " ", "inertia", " ", "matrices"}], "*)"}], 
   RowBox[{
    RowBox[{"For", "[", 
     RowBox[{
      RowBox[{"i", "=", "1"}], ",", 
      RowBox[{"i", "\[LessEqual]", 
       RowBox[{"Length", "[", "mm", "]"}]}], ",", 
      RowBox[{"i", "++"}], ",", 
      RowBox[{
       SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
       RowBox[{
        RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        " ", 
        RowBox[{"DiagonalMatrix", "[", 
         RowBox[{"{", 
          RowBox[{"1", ",", "1", ",", "1"}], "}"}], "]"}]}]}]}], "]"}], ";"}],
    "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
      RowBox[{"1", "-", "2"}], ":", " ", "left"}], ",", " ", 
     RowBox[{
      RowBox[{"4", "-", "5"}], ":", " ", "right"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "1"], "=", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        TagBox[
         RowBox[{"(", "\[NoBreak]", GridBox[{
            {"2.524082978466`*^6", "10713.891532`", 
             RowBox[{"-", "126075.629642`"}]},
            {"10713.891532`", "2.986642586756`*^6", "71009.473708`"},
            {
             RowBox[{"-", "126075.629642`"}], "71009.473708`", 
             "884479.151346`"}
           },
           
           GridBoxAlignment->{
            "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
             "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
           GridBoxSpacings->{"Columns" -> {
               Offset[0.27999999999999997`], {
                Offset[0.7]}, 
               Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
               Offset[0.2], {
                Offset[0.4]}, 
               Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
         Function[BoxForm`e$, 
          MatrixForm[BoxForm`e$]]], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "2"], "=", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        FormBox[
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"3.769899366786`*^6", "883.233008`", "111261.284918`"},
             {"883.233008`", "3.838716842066`*^6", "72389.846622`"},
             {"111261.284918`", "72389.846622`", "436115.120736`"}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]],
         TraditionalForm], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "3"], "=", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        TagBox[
         RowBox[{"(", "\[NoBreak]", GridBox[{
            {"8.642632927562952`*^7", 
             RowBox[{"-", "516556.06079885364`"}], 
             RowBox[{"-", "5.922561441486226`*^6"}]},
            {
             RowBox[{"-", "516556.060798854`"}], "9.021952557224008`*^7", 
             RowBox[{"-", "1.5119885900291622`*^7"}]},
            {
             RowBox[{"-", "5.922561441486225`*^6"}], 
             RowBox[{"-", "1.511988590029162`*^7"}], 
             "3.1728770928063706`*^7"}
           },
           
           GridBoxAlignment->{
            "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
             "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
           GridBoxSpacings->{"Columns" -> {
               Offset[0.27999999999999997`], {
                Offset[0.7]}, 
               Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
               Offset[0.2], {
                Offset[0.4]}, 
               Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
         Function[BoxForm`e$, 
          MatrixForm[BoxForm`e$]]], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "4"], "=", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalI]", "2"], "*", 
      RowBox[{"(", GridBox[{
         {"1", 
          RowBox[{"-", "1"}], "1"},
         {
          RowBox[{"-", "1"}], "1", 
          RowBox[{"-", "1"}]},
         {"1", 
          RowBox[{"-", "1"}], "1"}
        }], ")"}]}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "5"], "=", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalI]", "1"], "*", 
      RowBox[{"(", GridBox[{
         {"1", 
          RowBox[{"-", "1"}], "1"},
         {
          RowBox[{"-", "1"}], "1", 
          RowBox[{"-", "1"}]},
         {"1", 
          RowBox[{"-", "1"}], "1"}
        }], ")"}]}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"Needs", "[", "\"\<Units`\>\"", "]"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "Put", " ", "rotational", " ", "inertia", " ", "into", " ", "generalized",
      " ", "manipulator", " ", "inertia", " ", "and", " ", "add", " ", 
     "rotor", " ", "and", " ", "gearhead", " ", "inertia"}], "*)"}], 
   RowBox[{
    RowBox[{"For", "[", 
     RowBox[{
      RowBox[{"i", "=", "1"}], ",", 
      RowBox[{"i", "\[LessEqual]", 
       RowBox[{"Length", "[", "mm", "]"}]}], ",", 
      RowBox[{"i", "++"}], ",", 
      RowBox[{
       SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{"Join", "[", 
          RowBox[{
           SubscriptBox["\[ScriptCapitalM]", "i"], ",", 
           RowBox[{"Table", "[", 
            RowBox[{"0", ",", 
             RowBox[{"{", "3", "}"}], ",", 
             RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
         RowBox[{"Join", "[", 
          RowBox[{
           RowBox[{"Table", "[", 
            RowBox[{"0", ",", 
             RowBox[{"{", "3", "}"}], ",", 
             RowBox[{"{", "3", "}"}]}], "]"}], ",", 
           SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", 
         "1"}], "]"}]}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"p0", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "y"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{"\[Psi]", "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["p", "x"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["p", "y"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["p", "z"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SuperscriptBox["\[Psi]", "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}]}], 
      "}"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.5212045686835737`*^9, 3.521204606993765*^9}, {
   3.521204700067089*^9, 3.521204780986717*^9}, {3.5212145569110236`*^9, 
   3.5212145580280876`*^9}, {3.5212214210366507`*^9, 
   3.5212215159440784`*^9}, {3.521221549532*^9, 3.521221581995857*^9}, {
   3.5212225825050826`*^9, 3.5212225872403536`*^9}, {3.5212910875692406`*^9, 
   3.5212910900933847`*^9}, {3.5212911767343407`*^9, 
   3.5212911783804345`*^9}, {3.521291778077735*^9, 3.5212918161939154`*^9}, {
   3.521291985634607*^9, 3.521291991076918*^9}, {3.5212922081883364`*^9, 
   3.5212922159737816`*^9}, {3.5214849738620605`*^9, 3.521484978075301*^9}, {
   3.5214860083742313`*^9, 3.5214860164166913`*^9}, {3.522244545041182*^9, 
   3.522244546126244*^9}, {3.522244593734967*^9, 3.5222446610148153`*^9}, {
   3.522245182408637*^9, 3.5222451980385313`*^9}, {3.5222452480633926`*^9, 
   3.5222452884067*^9}, {3.5222453190144506`*^9, 3.522245320543538*^9}, {
   3.5222453895034823`*^9, 3.522245390398534*^9}, {3.5222454580864053`*^9, 
   3.5222454737583017`*^9}, {3.522508873288*^9, 3.522508878436*^9}, {
   3.52251692506606*^9, 3.5225169259961133`*^9}, {3.5226866514206*^9, 
   3.5226866515376*^9}, {3.5226870047536*^9, 3.5226870474105997`*^9}, {
   3.5226870857056*^9, 3.5226870879856*^9}, {3.5226871451626*^9, 
   3.5226871878106003`*^9}, 3.5230284498409986`*^9, {3.527367143201222*^9, 
   3.5273671538939753`*^9}, 3.5273671849756613`*^9, 3.527367344578813*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"generalized", " ", "coordinates"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"q", "=", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"dq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"ddq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"qe", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", 
       "q"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"dqe", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "location", " ", "and", " ", "direction", " ", "of", " ", "twists"}], 
    "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "0"], "=", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
      "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "px"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"PrismaticTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "pz"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"PrismaticTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q1"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q2"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q3"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q4"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q5"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"base", " ", "configuration"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "rc"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"-", 
            FractionBox["43", "10000"]}], ",", "0", ",", 
           RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"-", 
            FractionBox["43", "10000"]}], ",", "0", ",", 
           RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "rc"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "1"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "2"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "3"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "4"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "5"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "j"], "[", "\[Theta]", "]"}], "=", 
       RowBox[{"Simplify", "@", 
        RowBox[{"(", 
         RowBox[{"ForwardKinematics", "@@", 
          RowBox[{"Append", "[", 
           RowBox[{
            RowBox[{"Table", "[", 
             RowBox[{
              RowBox[{"{", 
               RowBox[{
                SubscriptBox["\[Xi]", 
                 RowBox[{"If", "[", 
                  RowBox[{
                   RowBox[{"i", "\[LessEqual]", "j"}], ",", 
                   RowBox[{"Symbol", "[", 
                    RowBox[{"\"\<q\>\"", "<>", 
                    RowBox[{"ToString", "[", "i", "]"}]}], "]"}], ",", "0"}], 
                  "]"}]], ",", 
                RowBox[{
                 SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}], "}"}], ",", 
              RowBox[{"{", 
               RowBox[{"i", ",", 
                RowBox[{"Length", "[", "q", "]"}]}], "}"}]}], "]"}], ",", 
            RowBox[{
             SubscriptBox["g", 
              SubscriptBox["sl", "j"]], "[", "0", "]"}]}], "]"}]}], ")"}]}]}],
       ",", 
      RowBox[{"{", 
       RowBox[{"j", ",", 
        RowBox[{"Length", "[", "mm", "]"}]}], "}"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}], "/.", "constsubs"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"change", " ", "LT", " ", "to", " ", "rT"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q5"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.504391169851249*^9, 3.504391191970865*^9}, {
   3.5129535326917257`*^9, 3.5129535355376797`*^9}, 3.513011937181409*^9, {
   3.513012138013254*^9, 3.513012139016991*^9}, 3.515858456069166*^9, {
   3.5158695182095623`*^9, 3.515869519316625*^9}, {3.5273673526577663`*^9, 
   3.5273673678729773`*^9}, 3.527367510176667*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", "aniplot", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"pos", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", "2"}], 
       "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "manipulator", " ", "inertia", " ", 
     "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]e", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["\[ScriptCapitalJ]", "i"], "\[Transpose]"}], ".", 
        SubscriptBox["\[ScriptCapitalM]", "i"], ".", 
        SubscriptBox["\[ScriptCapitalJ]", "i"]}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "project", " ", "out", " ", "the", " ", "generalized", " ", "coordinates",
      " ", "defining", " ", "the", " ", "position", " ", "of", " ", "the", 
     " ", "stance", " ", "foot", " ", "to", " ", "obtain", " ", "the", " ", 
     "reduced", " ", "\[ScriptCapitalD]", " ", "matrix"}], "*)"}], 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"3", ";;", "All"}], ",", 
         RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "Coriolis", " ", "matrix"}], "*)"}], 
   RowBox[{
    RowBox[{"\[ScriptCapitalC]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"InertiaToCoriolis", "[", 
       RowBox[{"\[ScriptCapitalD]", ",", 
        RowBox[{"Flatten", "[", "q", "]"}], ",", 
        RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "potential", " ", "energy", " ", "and", " ",
      "\[ScriptCapitalG]", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"V", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"g", " ", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalG]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}]}]}]], "Input",\

 CellChangeTimes->{{3.513875953621905*^9, 3.513876010689337*^9}, {
   3.513876043248599*^9, 3.51387610532577*^9}, {3.513876169675573*^9, 
   3.513876183706996*^9}, {3.513887655923167*^9, 3.513887658548081*^9}, {
   3.513888227999695*^9, 3.513888239050703*^9}, {3.5158587000371203`*^9, 
   3.51585871925922*^9}, {3.5158693371802073`*^9, 3.5158693660608597`*^9}, {
   3.5158696092147675`*^9, 3.5158696155961323`*^9}, {3.515886157784644*^9, 
   3.515886172869871*^9}, {3.516708254867365*^9, 3.5167082553383923`*^9}, {
   3.527367518307618*^9, 3.5273675270140142`*^9}, 3.5273677031466503`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
   "calculate", " ", "the", " ", "\[ScriptCapitalE]", " ", "matrix", " ", 
    "for", " ", "impact", " ", "and", " ", "the", " ", "guard"}], "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
        "\[RightDoubleBracket]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"\[ScriptCapitalE]", "/.", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".293"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
       ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".210"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
        ".923"}]}], "}"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"h", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"hdot", " ", "=", "  ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["gdot", "nsf"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"D", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "4"}], "]"}], "]"}], "/.", "p0"}], ",", "t"}], 
       "]"}], "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.513015087195842*^9, 3.513015090509893*^9}, {
   3.5167083039851747`*^9, 3.5167083075683794`*^9}, 3.516708384001751*^9, {
   3.5273677230197906`*^9, 3.527367759954082*^9}, {3.527367799151667*^9, 
   3.527367810348826*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"hip", " ", "position", " ", "related"}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"1", " ", "Hip", " ", "Position"}], "*)"}], "\[IndentingNewLine]", 
  
  RowBox[{
   RowBox[{
    RowBox[{"HipPos", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"2", " ", "Linearized", " ", "Hip", " ", "Position"}], "*)"}], 
   "\n", 
   RowBox[{
    RowBox[{"LHipPos", " ", "=", " ", 
     RowBox[{"Linearize", "[", 
      RowBox[{"HipPos", ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"non", "-", 
     RowBox[{"stance", " ", "slope", " ", "related"}]}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"nsslope", " ", "=", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
      "p0"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"2.", " ", "linearized", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"LinearNSslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nsslope", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"3.", " ", "hip", " ", "angle"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"HipAngle", " ", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"torso", " ", "related"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "Torso", " ", "non"}], "-", 
     RowBox[{"stance", " ", "thigh", " ", "angle"}]}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"theta4", " ", "=", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"2", " ", "Torso", " ", "Hip", " ", "angle"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"TorsoHipAngle", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"3", " ", "Torso", " ", "Non"}], "-", 
     RowBox[{"stance", " ", "Slope"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"nstorso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"4", " ", "Linearized", " ", "torso", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope", " ", "angle"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"LinearNStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nstorso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"5", " ", "Torso", " ", "stance", " ", "slope"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"storso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "6", " ", "Linearized", " ", "Torso", " ", "stance", " ", "slope"}], 
    "*)"}], "\n", 
   RowBox[{
    RowBox[{"LinearStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"storso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"7", " ", "COM"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"2", "*", 
               RowBox[{"(", 
                RowBox[{"mc", "+", "mt"}], ")"}]}], "+", "mh"}], ")"}]}]}]}], 
         "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"8", " ", "Linearized", " ", "COM"}], "*)"}], 
   RowBox[{
    RowBox[{
     SubscriptBox["Linearp", "COM"], "=", " ", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "COM"], "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], 
    ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.527268740044486*^9}, {
   3.527281326545467*^9, 3.527281336017424*^9}, 3.527282982629828*^9, {
   3.52736794853358*^9, 3.5273679739134893`*^9}, {3.527368012098797*^9, 
   3.5273680558134604`*^9}, {3.527368140597526*^9, 3.527368140978167*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
   "\"\</home/shu/workspace/Research/output_v2/models/NAO/\>\"", "<>", 
    "\"\<build_torso\>\""}], "]"}], ";"}]], "Input",
 CellChangeTimes->{{3.513876298225372*^9, 3.513876314895954*^9}, 
   3.5207161215675383`*^9, 3.527367774435699*^9, 3.5273688940470953`*^9, 
   3.527368976025791*^9, {3.5273692811108093`*^9, 3.527369283382406*^9}, {
   3.527369735661566*^9, 3.52736973625198*^9}, 3.527369922791857*^9, {
   3.527370173166801*^9, 3.527370177766226*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<nstorsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"nstorso", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaNStorsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LinearNStorso", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<storsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"storso", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaStorsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LinearStorso", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<hipposFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"HipPos", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaHipposFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LHipPos", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<nsslopeFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"nsslope", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaNSslopeFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LinearNSslope", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<comFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["p", "COM"], "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaCOMFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["Linearp", "COM"], " ", "/.", "p0"}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "/.", "statesubs"}]}], "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.52717692204714*^9, 3.5271769389718723`*^9}, {
   3.527177770383316*^9, 3.5271777843116817`*^9}, 3.5273698919196157`*^9}],

Cell[CellGroupData[{

Cell["PERL", "Subsection",
 CellChangeTimes->{{3.51353232609383*^9, 3.513532329542585*^9}}],

Cell["this next line assumes that perl is in your path", "Text",
 CellChangeTimes->{{3.513015230408543*^9, 3.513015241200741*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2mat_output.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}], 
   "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.504476276692404*^9, 3.504476301348216*^9}, {
   3.504478868125564*^9, 3.504478869967337*^9}, {3.504538702237328*^9, 
   3.504538704315194*^9}, {3.504631487786531*^9, 3.504631500259924*^9}, {
   3.504632214564033*^9, 3.504632215194798*^9}, {3.504635416937057*^9, 
   3.50463541912094*^9}, {3.504874639115547*^9, 3.504874639679282*^9}, 
   3.504876166859266*^9, {3.505578303460942*^9, 3.5055783039289427`*^9}, {
   3.512922890945364*^9, 3.512922891900902*^9}, {3.5129244660544987`*^9, 
   3.51292449464812*^9}, {3.512925468730114*^9, 3.512925469241001*^9}, {
   3.513011661780643*^9, 3.513011661926162*^9}, {3.51301497676734*^9, 
   3.513015001300451*^9}, {3.5158700422275343`*^9, 3.515870043613613*^9}, {
   3.515870412159693*^9, 3.5158704163519325`*^9}, {3.5158863672774124`*^9, 
   3.5158863722382207`*^9}, {3.5271773836362467`*^9, 3.527177386040779*^9}}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5155188337118206`*^9, 3.5155213252703295`*^9, {3.515540878198961*^9, 
   3.515540903604343*^9}, 3.5158407423966546`*^9, 3.5158416340670977`*^9, 
   3.5158498815118246`*^9, 3.515850149364145*^9, 3.5158505826739287`*^9, 
   3.5158508174343567`*^9, 3.5158510975593786`*^9, 3.515851653032055*^9, 
   3.5158525885305624`*^9, 3.5158527509368515`*^9, 3.5158527931192636`*^9, 
   3.51585495715904*^9, 3.5158550282321053`*^9, 3.5158552191920276`*^9, 
   3.515855253120968*^9, 3.515865262254135*^9, 3.515865638210639*^9, 
   3.51586592793721*^9, 3.51587048063961*^9, 3.5158730153755884`*^9, 
   3.5158739792467184`*^9, 3.515874203634553*^9, 3.515886694531988*^9, 
   3.5158874913514147`*^9, 3.515889070384316*^9, 3.5158900577352266`*^9, 
   3.515890252205177*^9, 3.5159482968094587`*^9, 3.515948944374403*^9, 
   3.515953395315056*^9, 3.515953710496083*^9, 3.5159537761788397`*^9, 
   3.5159726585821743`*^9, 3.5159857720472217`*^9, 3.516707085463479*^9, 
   3.516709802924909*^9, 3.5167112898219547`*^9, 3.5167126251093287`*^9, 
   3.516731910384804*^9, 3.51675156425562*^9, 3.517679232673436*^9, 
   3.517679752897191*^9, 3.517680087541332*^9, 3.5176859974273577`*^9, 
   3.5176980466975365`*^9, 3.5176984843215675`*^9, 3.517698597905064*^9, 
   3.520109553737687*^9, 3.520120022112444*^9, 3.520177851995036*^9, 
   3.5201861307705555`*^9, 3.520256997752266*^9, 3.5202590894559045`*^9, 
   3.520339215071435*^9, 3.5204373594555745`*^9, 3.5204379012015605`*^9, 
   3.5204436639822655`*^9, 3.520449216190834*^9, 3.520456964627019*^9, 
   3.520532857654432*^9, 3.5205334050077386`*^9, 3.520533641289253*^9, 
   3.520700208631287*^9, 3.520713025182435*^9, 3.520715993050187*^9, 
   3.520716372698902*^9, 3.5207164419848647`*^9, 3.520720498096861*^9, 
   3.520734731372958*^9, 3.5207350240366974`*^9, 3.520735233941703*^9, 
   3.5207876835786457`*^9, 3.520788877843954*^9, 3.5208972402433395`*^9, 
   3.5213037296170855`*^9, 3.5213038482928734`*^9, 3.52148604821651*^9, 
   3.521487916633377*^9, 3.5222451196790495`*^9, 3.522246794946869*^9, 
   3.52224990060228*^9, 3.522330715643431*^9, 3.5223566127951155`*^9, 
   3.5223569178824873`*^9, 3.5224296564487324`*^9, 3.522510361860586*^9, 
   3.5225118733730397`*^9, 3.522708144818318*^9, 3.5227751575219574`*^9, 
   3.522776845738859*^9, 3.5230285840646763`*^9, 3.5230306390121126`*^9, 
   3.523031339818296*^9, 3.5235572703774643`*^9, 3.52372487579274*^9, 
   3.5237265671694813`*^9, 3.5239837459472656`*^9, 3.5239849930135937`*^9, 
   3.5239893079543943`*^9, 3.5239903317719536`*^9, 3.523992025216813*^9, 
   3.5239921368791995`*^9, 3.5239948186215863`*^9, 3.5239949284198666`*^9, 
   3.5239950392682066`*^9, 3.5242481412858143`*^9, 3.524248528452959*^9, 
   3.5242499977068315`*^9, 3.5244001128388147`*^9, 3.5244002976151495`*^9, 
   3.5244062695321836`*^9, 3.524406550370155*^9, 3.5244067850253863`*^9, 
   3.524416267410578*^9, 3.524417771154109*^9, 3.5244178686872463`*^9, 
   3.5244183936479816`*^9, 3.524418700642165*^9, 3.524422224071623*^9, 
   3.5244223112816105`*^9, 3.524422752104824*^9, 3.5244238419211583`*^9, 
   3.524442216849144*^9, 3.524442742387203*^9, 3.5244438493725195`*^9, 
   3.5244456864925966`*^9, 3.5244458645247793`*^9, 3.5244468702188396`*^9, 
   3.5245024335606823`*^9, 3.5245119471381826`*^9, 3.5245883990742893`*^9, 
   3.525090287048571*^9, 3.525091695188668*^9, 3.5250919519797196`*^9, 
   3.5250921900848627`*^9, 3.5250924453299904`*^9, 3.525092571060454*^9, 
   3.52509268956332*^9, 3.5250928935899105`*^9, 3.5250932069259415`*^9, 
   3.525098766927955*^9, 3.5254479897919855`*^9, 3.525461216302328*^9, 
   3.5255182690806684`*^9, 3.525824548871134*^9, 3.527177429714081*^9, 
   3.527177710324808*^9, 3.5271838004534616`*^9, 3.5273332498298492`*^9, 
   3.527334075596567*^9, 3.527370198217165*^9}]
}, Open  ]]
}, Open  ]]
},
WindowSize->{959, 1024},
WindowMargins->{{Automatic, 67}, {Automatic, -8}},
ShowSelection->True,
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[545, 20, 810, 18, 126, "Input"],
Cell[1358, 40, 12237, 315, 710, "Input"],
Cell[13598, 357, 23360, 691, 1063, "Input"],
Cell[36961, 1050, 5605, 144, 315, "Input"],
Cell[42569, 1196, 4760, 140, 185, "Input"],
Cell[47332, 1338, 9948, 274, 687, "Input"],
Cell[57283, 1614, 527, 10, 30, "Input"],
Cell[57813, 1626, 4998, 149, 810, "Input"],
Cell[CellGroupData[{
Cell[62836, 1779, 91, 1, 37, "Subsection"],
Cell[62930, 1782, 130, 1, 30, "Text"],
Cell[CellGroupData[{
Cell[63085, 1787, 1269, 23, 69, "Input"],
Cell[64357, 1812, 3851, 54, 31, "Output"]
}, Open  ]]
}, Open  ]]
}
]
*)

(* End of internal cache information *)
